﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_ObjectIdentification.cs
 * \brief	This file contains the ObjectIdentification function that 
 *  
 * This file contains the ObjectIdentification function that 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2010-01-11
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ                         2010-01-11 \n
 *      First implementation of the M44_CompareReconstr.
 *      
 * - Version 0.02:   Gonzalo RODRIGUEZ                          2010-02-08 \n
 *      Change the iteration limits
 *      
/* ######################################################################### */
using System;
/*This is for using lists*/
using System.Collections;
using System.Collections.Generic;
using System.Text;
/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Basis;
using Interfaces.Complex;
/*Using the C Sharp Matrix Library*/
using NMRS.CSML;
/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_CompareReconst
     * 
     *  \brief  This is the class of the M44_CompareReconstr function.
     * 
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_CompareReconst
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static void CompareReconst(M44_auxSlamStr.cl_auxMapCell LocalMapCell, 
                                        M44_auxSlamStr.cl_auxMapCell LocalMapCell_ant,
                                        short n_matrix, short m_matrix)
         * 
         *  \brief 
         *  
         *  \param[in] M44_auxSlamStr.cl_auxMapCell LocalMapCell
         *  \param[in] M44_auxSlamStr.cl_auxMapCell LocalMapCell_ant
         *  
         *  \param[out] bool[,] MovementMap   Matrix indicating where in the map has been movement 
         i*
         */
        /* ------------------------------------------------------------------------- */
        public static bool[,] CompareReconst(aux_MapCell LocalMapCell, 
                                        aux_MapCell LocalMapCell_ant)
        {
            double MIN_CONF = 2;
            bool isMovement = false;
            bool[,] MovementMap = new bool[LocalMapCell.us_Nrows, LocalMapCell.us_Ncols];
            float Dif_X, Dif_Y;
            for (int i = 0; i < LocalMapCell.us_Nrows; i++)
            {
                for (int j = 0; j < LocalMapCell.us_Ncols; j++)
                {
                    isMovement = false;
                    if (LocalMapCell.Confidence[i, j] > MIN_CONF && LocalMapCell_ant.Confidence[i, j] > MIN_CONF)
                    {
                        //height check for differences more than 0.5 meters
                        if ((Math.Abs(LocalMapCell.Measured_Heights[i, j] - LocalMapCell_ant.Measured_Heights[i, j]) > 0.5))
                        {
                            Dif_X = Math.Abs(LocalMapCell.Measured_Gradient_X[i,j]-LocalMapCell_ant.Measured_Gradient_X[i,j]);
                            Dif_Y = Math.Abs(LocalMapCell.Measured_Gradient_Y[i,j]-LocalMapCell_ant.Measured_Gradient_Y[i,j]);
                            if (Dif_X>0 && Dif_X<40 && Dif_Y>0 && Dif_Y<40)
                               isMovement = true;
                        }
                    }
                    MovementMap[i, j] = isMovement;
                }
            }
            return MovementMap;
        }
    }
}